'''
Created on Aug 5, 2019

@author: modys
'''

if __name__ == '__main__':
    pass

from dynamicModel import Vehicle
from pid import PID
     
sample_time = 0.01;
time_end = 300;

model = Vehicle(time_end,sample_time);
controller = PID(time_end,sample_time,Kp=12.0, Ki=0.3, Kd=0.0);
controller.output_limits = (0.0,100.0);

model.reset();
model.setEngineCfg();
model.setVehicleCfg();
model.setFrictionCoff();
model.setTireForces();

# throttle value
throttle = 0.0
 
# incline angle (in radians)
alpha = 0
 
for i in range(model.getDataLength()):
    
    throttle = controller.update(25.0,model.getVelocity(),i)/100.0;
    model.step(throttle, alpha, i);
    
model.plotResults();
controller.plotResults ();
    